﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace Flight
{
    public class Helicopter : Aircraft
    {
        private Vector3 vel; // pixels/second
        //private float heading, pitch; // degrees
        private float yaw; // degrees/second
        private float power; // pixels/second/second

        private bool onGround;

        public readonly float ROTOR_PITCH; // degrees

        public readonly float AIR_FRICTION;
        public readonly float GROUND_DECELERATION;
        public readonly float BRAKING_DECELERATION;

        public readonly float MAX_GROUND_BACKPITCH; // degrees

        public readonly float MAX_GROUND_YAW_SPEED; // pixels/second

        public readonly float MAX_POWER; // pixels/second/second
        public readonly float POWER_RESPONSE;

        //public readonly float CENTER_Z; // pixels

        public readonly float YAW_DAMPING;

        public readonly float ROLL_SCALE;

        public readonly float GROUND_PITCH_FRICTION;

        public readonly float MAX_ROLL; // degrees
        public readonly float MAX_YAW; // degrees/second
        public readonly float YAW_ACCEL; // degrees/second/second
        public readonly float PITCH_VEL; // degrees/second

        public readonly float MAX_SPEED; // pixels/second

        public Helicopter(Vector3 loc, String name, float heading, float vel, Model model)
            : base(model, 0, 0, new HelicopterAutopilot(), loc, heading, 0)
        {
            Settings settings = new Settings(name + "/settings.txt");

            //Sound.loop(name + "/" + settings.get("sound")[0]);

            // so that it doesn't warn us about not using these properties
            settings.has("model");
            settings.has("sound");

            //this.loc = loc;
            this.vel = Sphere.degrees(heading, 0) * vel;

            this.yaw = 0;
            this.power = 0;

            this.ROTOR_PITCH = settings.get<float>("rotor-pitch");

            this.AIR_FRICTION = settings.get<float>("air-friction");
            this.GROUND_DECELERATION = settings.get<float>("ground-deceleration");
            this.BRAKING_DECELERATION = settings.get<float>("braking-deceleration");

            this.MAX_GROUND_BACKPITCH = settings.get<float>("max-ground-backpitch");

            this.GROUND_PITCH_FRICTION = settings.get<float>("ground-pitch-friction");

            this.MAX_GROUND_YAW_SPEED = settings.get<float>("ground-max-yaw-speed");

            this.MAX_POWER = settings.get<float>("max-power");
            this.POWER_RESPONSE = settings.get<float>("power-response");

            this.VIEW_Z = settings.get<float>("view-z");
            this.VIEW_DISTANCE = settings.get<float>("view-distance");
            //this.CENTER_Z = settings.get<float>("center-z");

            this.YAW_DAMPING = settings.get<float>("yaw-damping");

            this.ROLL_SCALE = settings.get<float>("roll-scale");

            this.MAX_ROLL = settings.get<float>("max-roll");
            this.MAX_YAW = settings.get<float>("max-yaw");
            this.YAW_ACCEL = settings.get<float>("yaw-accel");
            this.PITCH_VEL = settings.get<float>("pitch-velocity");

            this.MAX_SPEED = settings.get<float>("max-speed");

            settings.listUnusedSettings();
        }
        public Helicopter(Vector3 loc, String name, float heading, float vel)
            : this(loc, name, heading, vel,
            FlightGame.game.getModel(new Settings(name + "/settings.txt").get<string>("model")))
        {
        }

        public float getRoll()
        {
            return onGround ? 0 : computeAirRoll(yaw);
        }
        private float computeAirRoll(float yaw)
        {
            if (yaw >= 0)
                return MAX_ROLL * (1 - (float)Math.Exp(-yaw * ROLL_SCALE));
            //else return -computeAirRoll(-yaw);
            else return MAX_ROLL * ((float)Math.Exp(yaw * ROLL_SCALE) - 1);
        }

        // yawInput, pitchInput are in range [-1, 1]
        // powerInput is in range [0, 1]
        protected override void no_autopilot_update(float frameTime, ControlInputs inputs)
        {
            float yawInput = inputs.yaw,
                  pitchInput = inputs.pitch,
                  powerInput = inputs.power;
            bool isBraking = inputs.braking;

            this.yaw *= 1 - YAW_DAMPING * frameTime;// * (1 - PApplet.abs(yawInput));

            if (onGround)
            {
                // no yawing above limit
                if (this.vel.LengthSquared() >= MAX_GROUND_YAW_SPEED * MAX_GROUND_YAW_SPEED)
                    this.yaw = 0;
                // below limit, yaw is linear with speed (decreasing)
                else if (this.vel.LengthSquared() >= 3 * 3)
                    this.yaw = yawInput * MAX_YAW * (MAX_GROUND_YAW_SPEED - vel.Length() - 3) / MAX_GROUND_YAW_SPEED;
                else this.yaw = yawInput * MAX_YAW * vel.Length() / 3;
            }
            else this.yaw = MathHelper.Clamp(this.yaw + yawInput * YAW_ACCEL * frameTime, -MAX_YAW, MAX_YAW);

            // TODO: add a pitch-velocity value
            this.pitch += pitchInput * PITCH_VEL * frameTime;
            this.heading -= this.yaw * frameTime;
            float xySpeed = (float)Math.Sqrt(Math.Abs(vel.X * vel.X + vel.Y * vel.Y));
            vel.X = (float)Math.Cos(MathHelper.ToRadians(heading)) * xySpeed;
            vel.Y = (float)Math.Sin(MathHelper.ToRadians(heading)) * xySpeed;

            this.power += (powerInput * MAX_POWER - this.power) * POWER_RESPONSE * frameTime;

            if (onGround && vel.LengthSquared() > 0) // don't bother decreasing speed if we are already still
            {
                float newMag = vel.Length();
                newMag -= GROUND_DECELERATION * frameTime;
                if (isBraking) newMag -= BRAKING_DECELERATION * frameTime;
                if (newMag < 0) newMag = 0;
                vel *= newMag / vel.Length();
            }
            vel *= 1 - AIR_FRICTION * frameTime;

            this.heading = mod(this.heading, 360);
            this.pitch = mod(this.pitch, 360);

            if (onGround)
            {
                if (this.pitch < 180)
                    this.pitch *= 1 - GROUND_PITCH_FRICTION * frameTime;
                else this.pitch -= (this.pitch - 180) * GROUND_PITCH_FRICTION * frameTime;
            }

            Vector3 up = Sphere.degrees(heading, pitch + 90);
            Console.WriteLine(Math.Sqrt(Math.Abs(up.X * up.X + up.Y * up.Y)) + "   " + up.Z);

            vel.Z -= FlightGame.GRAVITY * frameTime;
            vel += up * power * frameTime;

            loc += vel * frameTime;

            if (loc.Z < 0)
            {
                onGround = true;

                //			if (vel.z < 0)
                //				vel.z = 0;
                vel.Z -= loc.Z / frameTime;
                loc.Z = 0;

                if (pitch > 45 && pitch < 360 - 20)
                    Console.WriteLine("crash!");
                if (pitch > 360 - 20)
                {
                    pitch = 0;
                }
                else if (pitch > MAX_GROUND_BACKPITCH)
                    pitch = MAX_GROUND_BACKPITCH;
            }
            else onGround = false;

            float deltaAlt = vel.Z * frameTime;

            // vnew^2 = vold^2 - 2*a*d
            float inside = this.vel.LengthSquared() - 2 * FlightGame.GRAVITY * deltaAlt;
            if (inside < 0)
            {
                //System.out.println("deltaAlt = " + deltaAlt + ", inside = " + inside);
                inside = 0;
            }
            if (vel.LengthSquared() > 0)
                this.vel *= (float)Math.Sqrt(inside) / vel.Length();
        }
        private float mod(float n, float d)
        {
            if (n > 0) return n % d;
            else return d + n % d;
        }

        public override void draw()
        {
            //Game.proxy.pushMatrix();
            //{
            //    Game.proxy.translate(loc);
            //    Game.proxy.rotateZ(MathHelper.ToRadians(heading)); // heading
            //    //if (!onGround)
            //    Game.proxy.rotateY(MathHelper.ToRadians(pitch)); // pitch
            //    Game.proxy.rotateX(-MathHelper.ToRadians(getRoll())); // roll
            //    Game.proxy.shape(MODEL);
            //}
            //Game.proxy.popMatrix();

            // TODO: roll around the CENTER-Z translated axis
            FlightGame.game.drawModel(MODEL, 1f, heading, pitch, getRoll(), loc);
        }

        public float getVelocity() { return (float)Math.Sqrt(vel.X * vel.X + vel.Y * vel.Y); }
        public float getPower() { return power; }
    }
}
